#include "main.h"
#include "usart.h"
#include "gpio.h"

#include "mySerial.h"
#include "stdio.h"
#include "string.h"
#include "MPU6050.h"
#include "AttitudeSolver.h"
#include "MadgWick.h"
#include "Anonymity.h"

extern char data[30];
extern uint8_t dat;
extern uint pointer;

/* USER CODE END 0 */

int main() {
    HAL_Init();
    MX_GPIO_Init();
    MX_USART1_UART_Init();
    OSInit();
		MPU6050_Init();
		AttitudeSolver_Init(1/0.08, 0.3f); // 采样频率125Hz，增益为0.3
	  int count = 0; //用于Yaw值线性回归矫歿?
	  float ax, ay, az, gx, gy, gz;
    while (1) {

			MPU6050_GetAccelData(&ax, &ay, &az);
      MPU6050_GetGyroData(&gx, &gy, &gz);
			float Roll, Pitch, Yaw;
			AttitudeSolver_UpdateIMU(gx, gy, gz, ax, ay, az);	
			// 获取姿濁角
			AttitudeSolver_GetEulerAngles(&Roll, &Pitch, &Yaw,count++);
			SendToAno03(Roll, Pitch, Yaw);

    }
    
    return 0;
}

void Error_Handler(void)
{
  /* USER CODE BEGIN Error_Handler_Debug */
  /* User can add his own implementation to report the HAL error return state */
  __disable_irq();
  while (1)
  {
  }
  /* USER CODE END Error_Handler_Debug */
}